// x(n+1)=A*x(n)+B*w(n)
// y(n)=H*x(n)+v(n)
// A, B
// H, R(v)
// x(n+1)=A*x(n)
// Pexp(n+1)=A*P(n)*A^T+B
// K(n+1)=Pexp(n+1)H^T*(H*Pexp(n+1)*H^T+R)^-1
// P(n+1)=(E-K(n+1)*H)*Pexp(n+1)
//dt=0.018717;
dt = 0.1;

R_0 = 0.02;
R_1 = 0.7;

x0=[0;1;0];

P0=[R_0,3*R_0/(2*dt),R_0/(dt^2);
    3*R_0/(2*dt),13*R_0/(2*dt^2),6*R_0/(dt^3);
    R_0/(dt^2),6*R_0/(dt^3),6*R_0/(dt^4)];

A=[1,dt,dt*dt/2;0,1,dt;0,0,1];

B=[0.1,0,0;0,0.1,0;0,0,0.1];

H=[1,0,0;0,1,0];

R=[R_0,0;0,R_1];

function [xexp, Pexp]=predict_state(xn,Pn, A, B)
    xexp = A*xn;
    Pexp = A*Pn*A'+B;
endfunction
function [xf,Pf]=filter_predicted_state(xexp,Pexp,H,R,y)
    K=Pexp*H'*(H*Pexp*H'+R)^-1;
    Pf=(eye(3,3)-K*H)*Pexp;
    xf=xexp+K*(y-H*xexp);
endfunction

function [res]=test_kalman_filter(inputFile,outputFile,x0,P0,A,B,H,R)
    i = size(R);
    m_data = read(inputFile,-1,1+i(1,1));
    j = size(m_data);
    xn = x0;
    Pn = P0;
    for i=1:j(1,1)
        [xexp, Pexp]=predict_state(xn,Pn,A,B);
        k=m_data(i,2:j(1,2));
        [xn,Pn]=filter_predicted_state(xexp,Pexp,H,R,k');
        res(i,1)=m_data(i,1);
        res(i,2)=xn(1,1);res(i,3)=xn(2,1);res(i,4)=xn(3,1);
        res(i,5)=0;
    end
    write(outputFile,res);
endfunction
test_kalman_filter("measurements.txt","out1.txt",x0,P0,A,B,H,R);